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ABSTRACT 

This  thesis  applies  extended  Kalman  filtering  to  the  problem  of  estimating  frequency,  ampli- 
tude, and  phase  of  a  nonsinusoidal  periodic  signal  contaminated  by  additive  white,  Gaussian 
noise.  Parameters  will  be  estimated  up  to  the  mth  significant  harmonic  component.  It  also  gives  an 
approach  for  the  case  of  less  than  mth  significant  harmonic  components.  The  estimator  will  track 
the  signal's  fundamental  frequency,  amplitudes,  and  phases  while  these  parameters  are  changing 
slowly  over  time.  The  amplitudes  are  estimated  as  if  the  fundamental  frequency  estimate  is  cor- 
rect; the  frequency  and  the  phases  of  the  signal  are  estimated  as  if  the  amplitude  estimation  is  cor- 
rect. This  thesis  also  contains  tracking  and  the  capture  behavior  of  the  filter. 
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I.  INTRODUCTION 

The  detection,  estimation  and  tracking  of  signals  plays  a  significant  role  in  many 
aspects  of  military  and  civilian  operations.  The  Kalman  filter  has  been  used  in  tracking 
problems  for  many  years.  Its  power  comes  from  the  mathematical  foundation  of  statistical 
optimality.  We  investigate  the  behavior  of  the  extended  Kalman  filter  instead  of  using  a 
linear  Kalman  filter,  as  most  of  the  real  world  problems  are  non-linear.  This  thesis  studies 
the  detection  of  non-periodic  signals,  corrupted  by  zero  mean,  white,  Gaussian  noise  using 
an  extended  Kalman  filter  algorithm.  The  parameters  of  the  signals  such  as  fundamental 
frequency,  harmonic  amplitudes  and  phases,  are  considered  unknown  and  are  estimated  by 
the  algorithm. 

In  nature,  most  physical  signals  are  approximately  harmonic.  This  problem  therefore, 
has  applications  in  many  fields  including  those  of  Radar  and  Sonar.  An  example  of  this  is 
in  the  tracking  of  propeller  driven  platforms  for  submarines  and  helicopters.  The  sound  of 
a  rotary  engine  is  almost  periodic.  If  the  engine  speed  changes,  the  sound  of  rotation 
changes,  resulting  in  a  change  of  frequency.  The  amplitudes  and  phases  may  also  change 
slowly  over  time.  Knowing  the  frequency,  amplitude  and  the  phase  of  the  signal  may  allow 
identification  of  the  class  of  vessel  generating  the  sound.  The  frequency  of  the  detected 
sound  tells  the  speed  of  the  vessel.  The  amplitude  identifies  the  amount  of  vibration. 

Other  applications  include  the  estimation  of  harmonic  signal  parameters  in  the 
presence  of  noise  to  determine  a  radar's  modulated  pulse  repetition  frequency  or  to 
investigate  noisy  biological  signals  such  as  heart  wave  forms. 


This  thesis  is  organized  into  six  chapters.  Chapters  II  and  III  explain  the  development 
of  the  Kalman  filter  and  show  the  mathematical  derivation  of  the  extended  Kalman  filter 
algorithm.  Chapters  IV  and  V  model  the  physical  signal  and  show  the  simulation  of  each 
model.  Chapter  VI  gives  the  conclusion  of  the  report.  The  appendices  contain  program  code 
applicable  to  this  work. 


II.  KALMAN  FILTER 

The  object  of  this  chapter  is  to  show  the  development  of  the  Kalman  filter  for 
estimating  states  from  noisy  data.  The  Kalman  filter  has  the  desirable  quality  of 
maintaining  the  physical  meaning  of  the  system  dynamics  by  utilizing  a  state  space 
representation.  Here  the  state  space  model  is  used  as  the  basic  model. 

The  optimality  of  the  filter  holds  for  systems  which  are  linear  and  time-invariant 
(LTI),  and  corrupted  by  additive  white  Gaussian  noise  (AWGN).  This  chapter  develops  the 
Kalman  filter  equations  for  such  a  system.  The  development  follows  closely  those  given  by 
Lewis,  Gelb,  and  Candy  [Ref.  2,  3,  4]. 

A.    DEFINITION  OF  TERMS 

All  of  the  terms  used  throughout  these  chapters  are  defined  in  Table  2.1  and  will  be 
explained  as  they  occur  in  the  derivation. 

Terms  with  a  single  time  subscript  (i.e.,  xk)  refer  to  the  value  of  the  term  at  that  time. 
Terms  with  dual  subscripts  (i.e.,  xA  +  1|  *)  refer  to  the  value  of  the  term  at  the  time  of  the  first 
subscript  given  observations  through  the  second  subscript.  The  third  column  of  Table  2.1 
gives  the  dimension  for  each  entry. 


TABLE  2.1:  DEFINITION  OF  TERMS 

System  order:  ;' 

Observation  size:  m 

System  state:                                          xk  jxl 

Transpose  of  state                                  xTk  1  xj 

State  transition  matrix:                           O  ;'  xj 

State  excitation  noise:                            wk  jxl 

Observation:                                            zk  mxl 

Observation  noise:                                   vk  mxl 

State  estimate:                                        **+i|*  ;'xl 

Estimate  error:                                       *k  +  i\k  7X1 

Expected  value  of  the  error:                    E[xk+1^k]  jxl 

Error  covariance  matrix:                         ^a  +  ii*  +  i  JXJ 

Residual:                                                  rk  +  l  mxl 

B.    SYSTEM  DYNAMICS 

A  good  model  of  the  system  is  necessary  for  estimating  parameters  of  that  system.  The 
state  space  representation  of  our  linear  system  is  given  in  Equation  (2.1),  and  the  measure- 
ment process  is  modeled  as  in  Equation  (2.2). 

zk  =  Hxk  +  vk  (  2.2) 


The  physical  state  of  the  system  (amplitude,  frequency,  etc.)  is  described  by  x  and  the 
observed  parameters  (amplitude,  frequency,  etc.)  are  described  by  observation  z.  Since  the 
described  system  is  time  invariant,  both  O  and  H  are  independent  of  time. 

The  noise  processes  are  considered  as  stationary,  uncorrected,  zero-mean,  additive 
white  Gaussian  noise  (AWGN)  processes.  The  statistical  properties  of  the  noise  processes 
are  given  below 


E[wk]  =  0  (2.3) 

E[WjLOTk]  =  Q8.h  (2.4) 


wk~(0,Q)  (2.5) 

E[vk]  =  0  (2.6) 

E[v/k]  =RSjk  (2.7) 

vk~(0,R)  (2.8) 

ElwjuJ]  =  0  (2.9) 

where  8  is  the  kronecker  delta  function,  defined  by: 


8,A=  {\^J  =  kh  (2-10) 


The  matrices  Q  and  R  are  non-zero,  diagonal  noise  covariance  matrices,  which  denote 
the  power  of  the  noise  of  the  system. 

C.    RECURSIVE  SYSTEM 

Before  presenting  the  detail  of  the  Kalman  filter  equations,  we  will  first  introduce  the 
linear  and  recursive  forms  of  the  filter,  as  shown  in  Equations  (2.11)  and  (2.12). 


**  +  ii  *  -  K\xk\  *  (  2.1 1) 

**+ii  *+i  -  ^2**  +  i|  * +  Gzk  +  l  (2.12) 

K\  is  a  system  matrix,  K^  and  G  are  the  time-varying  weighting  matrices.  The  current 
estimate  is  a  combination  of  the  previous  estimate  and  the  current  observation. 

D.    DISCRETE  KALMAN  FILTER 

The  Kalman  filter  may  be  derived  by  optimizing  the  assumed  form  of  the  linear  esti- 
mator. An  optimal  system  is  generally  considered  to  be  any  system  which  either  minimizes 
a  cost  function  or  optimizes  a  performance  function. 

This  experiment  required  the  establishment  of  a  filter  that  gives  an  unbiased  estimate 
and  minimizes  error.  An  unbiased  estimate  has  an  expected  error  value  of  zero.  The  Con- 
stants K\  in  Equation  (2.1 1)  and  K^  in  Equation  (2.12)  are  chosen  to  make  the  estimate  un- 
biased. The  constant  G  is  chosen  to  minimize  a  cost  function  of  the  expected  error. 

Following  measurement,  an  equation  for  the  estimation  error  can  be  obtained  from 
Equation  (2. 12)  by  substitution  of  the  measurement  Equation  (2.2)  and  the  defining  the  re- 
lations as  (tilde  denotes  estimation  error): 

xk+i\  k  =  xk+i  ~**  +  i|  *+  i  (2.13) 

^A  +  llA  +  l    =  xk  +  l~  Xk  +  l\k  +  l-  (2.14) 

The  expected  values  satisfy 

E[ih+llk+l]  =E[£k+1]k]  =  0  (2.15) 


and 


**  +  l|*  +  l=     {K2  +  GH-I\Xk  +  \\k+K2Xk+Y\k  +  K2Vk-  (2-!6) 

By  taking  the  expected  value  of  both  sides  and  letting  E[xk  +  1]k]  =0,E[vk]  =  0,and 
E[xk+i\k+i]  =  °  (i-e-  unbiased  estimator),  the  term  in  square  brackets  in  Equation  (2.16) 
is  required  to  be  zero. 

K2  =  I-GH  (2.17) 

Combining  Equation  (2.14)  and  (2.17)  gives  the  estimator  the  form  of 

**+i|*+i=  [I-GH)xk  +  llk  +  Gzk+l  (2.18) 

or,  alternatively 

xk+i\  A  +  i  -  xk  +  u|  +  G  izk+  i  ~  Hxk+  i|  *1  •  (2.19) 

Following  the  same  procedure,  the  value  of  Kj  can  be  determined  by  substituting 
Equations  (2.1)  and  (2.11)  into  Equation  (2.13)  as  below: 

Xk  +  l\k  =  <i>Xk  +  Wk-K\xk\k~(^xk\k-  (  2-2°) 

By  taking  the  expected  value  of  both  sides  and  applying  Equations  (2.3)  and  (2.15), 
we  get 

tf^O  (2.21) 


so  that  Equation  (2.1 1)  can  be  written  in  the  form 


*k  +  i\k  =  ***!*•  (2.22) 


Equation  (2.22)  is  known  as  the  time  update  portion  of  the  algorithm  and  gives  the 
prediction  xk  +  llk  of  the  state  at  time  k+\,  along  with  the  associated  error  covariance 
^*  +  i|  *•  Also,  Equation  (2.19)  is  known  as  measurement  update  portion  which  provides  a 
correction  based  on  the  measurement  zk  + ,  at  time  k+ 1  to  yield  the  net  a  posteriori  estimate 
of  xk+  j  and  its  error  covariance  of  Pk+l,k+1.  This  is  known  as  predictor-corrector  formu- 
lation of  the  discrete  Kalman  filter. 

E.     ERROR  COVARIANCE  UPDATE 

To  obtain  a  measure  of  confidence  for  the  estimate,  we  need  to  find  the  error  covari- 
ance. The  error  covariance  matrices  are  defined  by  Equations  (2.23)  and  (2.24). 

^*  +  il  *  =  &  [**  +  i|  kxk  +  i\  *1  (2.23) 

^A  +  il^  +  i  =  E  [**+i|  k  +  ixk  +  \\  *+il  (2.24) 

The  corresponding  estimation  error  is  of  the  form 

**+i|*+i=  [I-GH]xk  +  1]h  +  Gvk.  (2.25) 

Rewriting  Equation  (2.24)  as 


S 


p*  +  i|*+i  =  E{(I-GH)ik  +  llk[il+ilka-GH)T+vTkGT\+GvkliTk  +  1]k(I-GH)T+vTkGT]} 

(  2.26) 
and  using  Equations  (2.23),  (2.7),  and  the  result  of  uncorrected  measurement  errors  we 
have, 

E[ik  +  l\kvl]  =E[vkiTk+llk]  =  0,  (2.27) 


and 


^*  +  H*  +  i  =  (I-GH)Pk  +  llk(I-GH)T+GRGT.  (2.28) 

The  error  covariance  matrix  gives  the  expected  magnitude  of  the  estimation  error. 

F.     OPTIMUM  CHOICE  OF  KALMAN  GAIN 

The  criterion  for  choosing  the  gain  is  to  minimize  a  weighted  scalar  sum  of  the  diag- 
onal elements  of  the  error  covariance  matrix.  We  will  choose  the  value  of  G  which  satisfies 
Equation  (2.29). 

G:minlG{Jife+1}   =  E[xk+1]k+1xl+l\k+1]  (2.29) 

Equation  (2.29)  can  be  written  in  terms  of  the  error  covariance  as 

G:min'G{J,  +  1}   =  trace  (P4+1|ik  +  1) .  (2.30) 

This  is  equivalent  to  minimizing  the  length  of  the  estimation  error  vector.  To  find  the 
value  of  the  gain  which  provides  a  minimum,  it  is  necessary  to  take  the  partial  derivative 


of  Jk  + !  with  respect  to  G  and  set  it  to  zero.  Taking  the  partial  derivative  of  Jk  + ,  requires 
the  use  of  the  matrices  A  and  £,  where  B  is  symmetric. 


4rtrace(ABAT)  =  2AB  (2.31) 

aA 


Inserting  Equation  (2.28)  into  Equation  (2.30)  and  applying  Equation  (2.31)  gives  the  value 


2(I-GH)Pk  +  llkHT+2GR  =  0.  (2.32) 


Solving  for  G,  we  have 

G  =  Pk+1]kHT[HPk  +  l]kHT  +  R]-1  (2.33) 

which  is  referred  to  as  the  Kalman  gain  matrix. 

G.    KALMAN  FILTER  EQUATIONS 

The  set  of  recursive  equations  in  Table  2.2  provide  the  time  varying  optimal  gain  ma- 
trix and  the  error  analysis  of  the  estimate. 
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TABLE  2.2:  KALMAN  FILTER  EQUATIONS 

Time  update  (effect  of  system  dynamics) 

Error  covariance: 

^*  +  n*  =  ^*i**T+Q  (2.34) 

Estimate: 

*k  +  i\k  =  ***|*  (2.35) 

Measurement  update  (effect  of  measurement  z  ) 

Error  covariance: 

^+H*=  [(P4+1|4)-1  +  flT2rlH]-1  (2.36) 

Residual: 

ek  +  \  =  zk  +  \-Hxk  +  \\k  (2.37) 

Estimate: 

**  + 1|  k+ 1  =  **  + 1|  *  +  Pk  + 1|  *+  i^  ^    (2*+  i  ~H*k  + 1)  (  2.38) 


Alternative  Measurement  Update  Equations: 


G  =  Pk  +  llkHT(HPk  +  l]kHT  +  R)-1  (2.39) 

P*  +  H*  =  (I-GH)Pk+1]k  (2.40) 

**  +  i|*+i  =  **+i|  k  +  G(z-Hxk+ 1)A)  (2.41) 


These  equations  require  the  following  initial  conditions,  xol  0  and  P0]  0 
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There  are  many  equivalent  formulations  for  Equations  (2-33)  and  (2-35).  Using  the 
matrix  inversion  lemma  the  latter  can  be  written  as 

P*+i|*+i  =  Pk.nk-Pk.iik^lHP^^+Rr'HP^,,  (2.42) 

Note,  Equation  (2.42)  requires  only  a  pxp  matrix  inversion.  Equation  (2.36)  requires  the 
inversion  of  two  vxv  matrices,  and  the  number  of  measurements  for/?  is  usually  less  than 
for  v.  If  |i*A+1|  J  =  0,  then  Equation  (2.44)  must  be  used. 

If  it  is  often  convenient  to  replace  the  measurement  update  equations  by  alternative 
equations  such  as  those  in  Table  2.1. 

The  Kalman  filter  gain  is  the  weighting  that  determines  the  influence  of  the  residual 
in  updating  the  estimate.  It  is  important  to  remember  that  the  equations  of  Table  2.2  are  only 
optimal  when  the  system  is  linear,  and  when  the  a  priori  knowledge  of  the  noise  is  avail- 
able. 
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III.  EXTENDED  KALMAN  FILTER 

In  this  section,  an  approximate  solution  to  the  non-linear  filtering  problem  defined 
below  is  developed.  This  solution  involves  the  linearization  of  a  non-linear  process 
traversing  about  a  reference  trajectory  and  the  modification  or  extension  of  the  linear 
Kalman  filter  algorithm  using  the  linearized  model. 

In  practice,  many  processes  are  non-linear  rather  than  linear.  Coupling  non-linearities 
with  noisy  data  makes  the  signal-processing  problem  a  challenging  one.  Instead  of 
extending  this  solution  to  the  continuous  case,  the  extended  Kalman  filter  algorithm  was 
developed  to  test  for  the  discrete  non-linear  system. [Ref.  2]  and  [Ref.  3]. 

A.  SYSTEM  DYNAMICS 

Let  us  discuss  the  non-linear  system  of  the  form 

**+i  =  *>xk  +  wk  (3.1) 

zk  +  i  =  h(xk  +  i)  +  u*+i>  O-2) 

where  wk  -  (0,  Q)  and  vk  -  (0,R)  are  white  noise  processes  uncorrected  with  each  other. 

B.  APPROXIMATE  MEASUREMENT  UPDATE 

In  order  to  find  a  measurement  update  that  can  be  conveniently  programmed,  h  (xk  +  x) 
is  expanded  into  a  Taylor  series  about  ik  + 1{  k,  an  a  priori  estimate  at  time  k+\. 
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h(xh+1)  =  h(*k+l\k)+M 

dx 


(*a  +  i  ~**+i|*)  +  higher  order  terms        (  3.3) 


X  =* 


*.ll  * 


Neglecting  the  higher  order  terms  (H.O.T.) 


h(xk+1)  =  h(xk+llk)+M: 

dx 


**  +  ii  k 


*  =  ■***! 


I   * 


(3.4) 


representing  the  Jacobian  as; 


H(x,k)  =  ^-h(x,k), 


(3.5) 


using  Equations  (3.4)  and  (3.5)  can  be  used  to  proceed  with  the  Kalman  filter  analysis. 

C.     APPROXIMATE  TIME  UPDATE 

To  obtain  a  complete  filtering  algorithm,  update  equations  that  account  for  measure- 
ment data  are  needed.  We  choose  a  linear,  recursive  set  of  equations  for  the  estimate, 


*A  +  1|A+1    -   a*  +  l|  k  +  Gk  +  1Zk+l, 


(3.6) 


where  the  vector  ak  and  the  gain  matrix  Gk  are  to  be  determined.  Proceeding  with 
arguments  similar  to  those  used  in  Chapter  II,  we  define  the  estimation  error  as 


*A  +  1|  A+  1  =xk+  1|  k  ~xk+  1  • 


(3.7) 
(3.8) 
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Equations  (3.6)  and  (3.7)  and  (3.8)  are  combined  with  Equation  (3.2)  to  produce  the 
following  expression  for  the  estimation  error: 

**+i|*  +  i  =  Aa*+i  +  G*  +  1/2(*A+1)  +  Gk  +  lvk+l  +  xk  +  llk-xk+1]k  (3.9) 

One  required  condition  is  that  the  estimate  is  unbiased.  Applying  this  requirement  to 
Equation  (3.9)  and  letting  E  [xk+ 1(  k]  =  E  [vk]  =  0,  we  obtain 

aA+i  +  <^  +  ]/i(x*+i,*)  -*a  +  i|a  =  0.  (  3.10) 

By  defining  the  residual  as  the  difference  between  the  observation  and  the  expected 
value  of  the  observation  in  Equation  3.1 1 

e*  =  i  =  «a  +  i-a(**+i|*)«  (  3-n) 

solving  Equation  (3.10)  for  aA+1,  and  substituting  the  result  into  Equation  (3.6),  and 
combining  it  with  Equation  (3.1 1)  yields  the  extended  Kalman  filter  estimate  equation 

**  +  i|  *+  i  =  **+  i|  k  +  Gk  +  lrk  +  l.  (3.12) 

D.     ESTIMATION  ERROR  COVARIANCE 

Using  the  definition  of  error  covariance 

^A+l|  A   =   £  [**  +  !!  ***  +  l|*l  (3.13) 
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and 


■T 


^A+ll  A  +  l    ~   &  [*A+1|  A  +  1*A+1|  A  +  l]  (3.14) 

the  error-update  equations  may  be  derived.  Taking  Equation  (3.10)  into  account,  Equation 
(3.9)  becomes 

**  +  H*  +  i  =  xk+1\k  +  Gk+1[h(xk+1)  -h(xk+1)]  +Gk+1vk+1.  (3.15) 

Using  this,  to  generate  the  error  covariance  Pk  +  l  in  terms  of  the  yet  undetermined 
Gk  +  1  we  can  write 


E{ik  +  1{k[h(xk  +  1)-h(xk  +  l)]T}GTk  +  1  + 
Gk+1E{[h(xk+1)-h(xk+l)]il+Mk]  +Gk+1Rk+lGl+1,       (3.16) 


where 


Rk  =  E[vkvTk) 


The  gain  Gk  +  1  is  now  selected  to  minimize  Pk  +  l.  Differentiating  Pk  +  1  with  respect 
to  GA  +  1  and  solving  for  Gk  +  1  results  in  the  desired  optimal  gain  matrix. 
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GA  +  1  =  E{(xk  +  l]k)  [h(xk+1)-h(xk+1)]T}x 
{E{[h(xk  +  1)-h(xk+1)]  [h(xk  +  ,)-h(xk  +  ,)]T}  +Rk  +  1}'\    (3.17) 

Substituting  this  into  Equation  (3.16)  and  simplifying  yields 

P*+H*  +  1  =Pk  +  i\k  +  Gk+1E{[h(xk+l)-h(xk  +  1)]iLi\k}-  (3.18) 

The  complete  linear  estimate  update  due  to  non-linear  measurement  is  given  by  Equa- 
tions (3.15),  (3.18)  and  (3.19).  However,  these  equations  are  impractical  to  implement  be- 
cause they  depend  on  conditional  moments  of  xk+l  in  computing  h  (xk  +  x) .  In  order  to  sim- 
plify the  computation,  we  will  expand  h(xk  +  ilk  +  i)  into  a  power  series  about  xk  +  Mk  as  fol- 
lows: 

^(*A+l|A+l)    =   ^  (*A  +  1|  k)  +  ^(*A+1|  *)  (**+]|  A+l  -*A  +  1|  k)  +  •••  (3.19) 


where 


#(*a  +  h*)  =:  4r.hW 


dx 


*  =  **  + 1 1  * 


Truncating  the  above  series  after  the  first  two  terms,  substituting  the  resulting  approx- 
imation into  Equations  (3.18),  (3.19),  and  carrying  out  the  indicated  expectation  operations 
results  in  a  measurement  error  covariance  update  as  follows: 

Pk+i\k  +  i  =  U-Gk  +  1H(xk  +  llk)]Pk  +  l]k.  (3.20) 
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This  equation  represents  an  approximate,  linear  measurement  update  for  the  error  co- 
variance.  The  residual  is  computed  using  the  non-linear  measurement  function  h  (x)  eval- 
uated about  the  a  priori  estimate  xk  +  1\  k.  The  error  covariance  is  found  using  the  Jacobian 
matrix. 

TABLE  3.1:  EXTENDED  KALMAN  FILTER  EQUATIONS 
System  model  and  measurement  model: 

x  =  f(x,t)+w(t)  (3.21) 

zk  =  h(x(k))  +vk  (3.22) 

w(t)~  (0,Q),vk~(0,R)  (3.23) 

Time  update: 
Estimate  (state  prediction): 

*A  +  1|A  =  fXk\k  (  3.24) 

Jacobians: 

*(*,*)  =#/(*,*)  (3.25) 

dx 

H(x)  =  -$-h(x,k)  (3.26) 

dx 

Error  covariance  (covariance  prediction): 

Measurement  update: 
Kalman  gain: 

G*  +  i  =  ^*+i|  kH  (**  +  i|*)  [H(*k  +  i\  k)Pk  +  \\  A  (xk  +  i\k)  +R]  (3.28) 

Error  covariance  (covariance  correction): 

PA-H|A+1  =   17-ga  +  i#(*a  +  i|a)]pa+i|a  (3.29) 

Estimate  (state  correction): 

*k  +  i\k  +  \  =  £k  +  i\k  +  Gk+1[zk  +  l-h(xk+1]k)]  (3.30) 
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The  covariance  and  gain  equations  are  identical  to  those  in  Table  2.2,  but  with  the  Ja- 
cobian  values  A  and  H  linearized  about  xk  +  ilk-  Note  that  P  and  G  are  now  functions  of  the 
current  state  estimate,  which  is  a  conditional  mean  and  therefore  a  single  realization  of  a 
stochastic  process.  It  also  should  be  realized  that  the  measurement  times  do  not  need  to  be 
equally  spaced.  The  time  update  is  performed  over  any  interval  during  which  no  data  are 
available.  When  data  become  available,  a  measurement  update  is  performed.  This  means 
that  an  indication  of  missing  measurements  can  be  obtained,  and  pure  prediction  in  the  ab- 
sence of  data  can  easily  be  achieved  by  using  the  extended  Kalman  filter. 

Higher-order  approximations  to  the  optimal  non-linear  updates  can  also  be  derived 
by  retaining  higher-order  terms  in  the  Taylor  series  expansions. 
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IV.  POLAR  MODEL  OF  THE  SYSTEM 

Consider  an  approximately  periodic,  non-sinusoidal  signal,  in  additive  white 
Gaussian  noise.  A  non-sinusoidal  signal  may  be  considered  to  consist  of  an  infinite  number 
of  sinusoidal  components.  Three  sets  of  parameters  can  characterize  the  signal:  the 
fundamental  frequency,  the  amplitude  of  each  harmonic  component,  and  the  phase  of  each 
harmonic  component. 

The  signal  is  not  exactly  periodic  since  frequencies,  amplitudes  and  phases  change 
slowly  over  time.  We  will  keep  the  same  notation  as  defined  in  Nehorai  and  Porat  [Ref.  6] 
and  Parker  and  Anderson  [Ref.  7].  For  this  purpose,  we  will  first  take  a  periodic  signal  y  (0 
with  a  zero  d.c.  component.  A  Fourier  series  representation  of  this  signal  can  be  written  as: 

yw  =  ^rksin(kWft  +  \)  (4.1) 

k=  1 

In  this  paper  a  discrete  time  domain  (i.e.  t  =  0,1,2,...)  rather  than  a  continuous  domain 
will  be  used.  As  our  signal  y(t)  is  not  exactly  periodic,  but  has  a  slowly  time  varying 
frequency  wf,  amplitudes  rk,  and  phases  tyk,  we  can  state 


wf  =  wf(t)  ( 4.2) 

r  =  rk(t)  (4.3) 

<k  =  4>4  (0  (  4.4) 
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where  wf(t) ,  rk(t)  and  <t>A(f)  are  nearly  constant  over  a  few  periods  of  y(t) .  In  other 
words; 


\wf{t+\)  -wAt)\ 

'=0  (4.5) 


ny<0 

Vk 

(*+l)-r, 

(0| 

wr(0 

!♦* 

(*+!)-«>, 

M)\ 

wAt) 


=  0  (4.6) 

^0  (4.7) 


In  the  model  of  y(t),  the  quantities  wf(t) ,  rk(t)  and  $k(t)  are  the  instantaneous 
frequency,  amplitudes,  and  phases  of  the  signal.  We  will  call  the  model  given  in  Equation 
(4.1)  the  polar  model  and  the  model  given  in  Chapter  V  the  rectangular  model  [Ref.7]. 

We  assume  for  both  models  that  the  signal  y(t)  is  corrupted  by  noise.  The 
measurements  are  given  by 

z(0  =  y(t)+v(t).  (4.8) 

The  task  is  to  estimate  the  values  rx  (t)  ,...,rm  (t)  ,wf(t) ,  <t>j  (t)  ,—4>m  (0  from  the  mea- 
surements, where  m  denotes  the  number  of  the  significant  harmonics.  Parameters  are  only 
estimated  up  to  mth  harmonics.  The  higher  harmonics  are  assumed  to  be  negligible.  A  total 
of  2/w+l  parameters  must  be  estimated. 

As  explained  in  Parker  and  Anderson  [Ref.7],  we  are  also  estimating  amplitudes  as 
well  as  the  fundamental  frequency.  This  is  required  to  establish  an  estimator  that  uses  both 
the  energy  in  the  fundamental  and  the  energy  in  the  higher  harmonics  to  estimate  the  fre- 
quency of  the  signal.  The  information  about  the  frequency  contained  in  any  harmonic  de- 
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pends  on  the  energy  in  that  harmonic.  If  a  particular  harmonic  component  is  strong,  then 
the  estimator  of  the  frequency  components  should  give  more  weight  to  the  information 
available  in  the  strong  harmonic  and  less  weight  to  the  information  available  in  the  weak 
harmonics. 

Estimation  of  the  harmonic  amplitude  also  assists  in  estimating  the  frequency.  The  es- 
timator determines  the  frequency  by  first  estimating  the  harmonic  amplitudes.  Knowledge 
of  the  frequency  and  the  phases  in  the  polar  model  also  assists  in  the  calculation  of  the  har- 
monic amplitudes. 

A.    THE  ESTIMATOR 

Given  a  noisy  measurement  sequence  and  the  associated  models  as  shown  in  Fig. 
4.1[Ref.4],  the  Kalman  filter  can  be  thought  of  as  an  estimator  that  produces  three  types  of 
outputs.  The  first  filter  can  be  named  as  a  state  estimator  or  reconstructor.  It  reconstructs 
estimates  of  the  state  x(t)  from  noisy  measurements  y  (t) .  This  kind  of  model  may  be 
thought  of  as  the  means  to  implicitly  extract  x  (t)  from  y  (t) . 

The  second  filter,  may  be  thought  of  as  a  measurement  filter  that  accepts  noisy  se- 
quences of  input  and  produces  a  filtered  sequence  of  output.  Finally,  the  estimator  can  be 
thought  as  a  whitening  filter  that  accepts  noisy  correlated  measurements  and  produces  un- 
corrected or  white  residues  e  (t)  =  z  (t)  -z  (t\t  -  1) .  For  our  purposes,  we  will  concentrate 
on  the  measurement  filter  form. 
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y{t) 


y(t) 


y(t) 


State 
estimator 

Measurement 
filter 

Whitening 
filter 

x(t\t) 


y(t\t) 


e(t) 


Figure  4.1  Various  representations  of  the  Kalman  filter 

As  mentioned  before,  the  extended  Kalman  filter  will  be  used  to  estimate  frequency, 
phases,  and  amplitudes  of  an  almost  periodic  signal  imbedded  in  noise.  First,  we  need  a 
state  space  representation  of  the  signal  defined  in  Equation  (4.1).  Therefore, 


x(t  +  l)  =  Ox(t)  +w(t) 

z(t)  =  y(t)  +v(t) 
z(t)  =  h(x(t))+v(t) 


(4.9) 
(4.10) 
(4.11) 


and 


x(t)  =[r,(0,r2(0,.-.,rm(0,  W/r(O,<l>,(O,<l>2(O,...,0m(O  1  (4.12) 


and 
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o  = 


~Im 

0  0 

0 

1   0 

0 

0Im_ 

(4.13) 


where,  Im  is  a  mth  order  identity  matrix, 


h(x(t))   =   y\ r k (t)  sin  (kw ft +  <$>k) 


k  =  i 


and  w  (t)  is  white  Gaussian  noise,  with  a  zero  mean  and  a  variance 


(4.14) 


E[w(t)w  *(*)]  =  Q. 


(4.15) 


The  observation  noise  v  (t)  is  also  white  Gaussian  noise,  with  zero  mean  and  has  a 
variance 


E[v(t)v7  (t)]  =  R 


(4.16) 
(4.17) 


and  is  uncorrected  with  w  (t) . 


E[w(t)v(t)]  =  0 


(4.18) 


We  will  have  a  Q  matrix  which  is  diagonal.  Its  value  will  be  given  in  the  simulation 
section.  From  Equation  (4.9),  it  can  be  concluded  that  the  harmonic  amplitudes  evolve 
randomly  over  time.  Also,  the  same  argument  is  true  for  wAt) ,  the  fundamental  frequency, 
and  the  §k  (t)  phases  of  the  signal.  The  rate  of  the  random  walk  will  be  determined  by  the 
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diagonal  Q  matrix.  A  zero  Q  matrix  will  correspond  to  constant  amplitude,  frequency  and 
phase.  In  order  to  estimate  x(t\t)  or  x  (t\t  -  1)  of  x  (t)  from  the  measurement  z  (t) ,  the  ex- 
tended Kalman  filter  will  be  applied.  Here  x(t\t)  denotes  the  estimation  of  x(t) ,  given 

measurements  [z (t) |  i  =  0,  l,  2 t)  up  to  and  including  time  t.  The  value  x(t\t-  1)  is  an 

estimate  of  x  (t) ,  given  z  (x)  up  to  time  t- 1 . 


x(t\t)  =  x(t\t-l)  +G(t)  [z(t)  -h(x(t\t-l)) 
x{t+l\t)  =  OAUIO 
G(t)  =  P{t)Hr{t)  (H(t)P(t)HT(t)  +R)~i 
P(t  +  1)  =  *[P(0  -G(t)H(t)P(t)]<t>T+Q 


(4.19) 

(  4.20) 
(4.21) 
(  4.22) 


where  H(t)  is  the  Jacobian  of  hit) .  That  is: 


H(t) 


dh(x(t\t-  1)) 
di(t\t-  1) 


H(t)  = 


sin  (uy(*|f-  1)*  +  <j>j(*|<-  1)) 
sin  (^(<|t-  1)*  +  <|>2U|*-  1)) 

sin  {wf(t\  t  -  l)t  +  $m(t\t-l)) 

3(«U-  1) 

M*l*~  1)  cos  (&r(*U  -  1)  *  +  0i  CI  *  -  1)) 

f2  (*|  *  -  1)  cos  (uy(*|  * - 1) t  +  02  (t\  t-1)) 
rm  (t\  t  -  1)  cos  (wf(t\  t-l)t  +  $3(t\t-l)) 


(  4.23) 
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where, 


3(*|*-1)  =  \f-ktcos(ti)f(t\t-l)t  +  $k(t\t-l)) 


k=  1 


and  the  initial  values  are 

x(0)  =  E[x(0)}  =  x(0) 

and 

P(0)  =  E[(x(0)  -x(0))(*(0)  -x(0))r]. 

B.     SIMULATIONS 

In  the  simulation  we  let 

z(t)   =  rjsin  (27c0.04*  +  0.01)+r2sin  (4n0.04t  +  0.01)  +r3sin  (6n0.04t  +  0.01)  + 
r4(87c0.04*  +  0.01)+u(O  (4.24) 

where  y(0  is  a  zero  mean,  white  Gaussian  noise  process  with  a  variance  of  0.1.  The 
fundamental  frequency  of  the  signal  wf  =  27:0.04  and  the  amplitudes  of  the  signal  rk  are 
constant  over  time.  We  use  the  amplitudes  as  written  below. 

rk  =  r-^  k  =  2,3,4,  (4.25) 
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where  rx  is  chosen  for  a  desired  signal  to  noise  ratio  as  given  by 


/YV-    1     2^ 


SNR  =  10  log 


-r, 


*  =  i 


0.1 


( 4.26) 


For  the  simulations,  the  filter  program  was  written  in  MATLAB  (See  Appendix  A), 
with  the  following  initial  conditions: 


MO)  =  2.4 

( 4.27) 

MO)  =  o 

(4.28) 

MO)  =  o 

(4.29) 

MO)  =  o 

( 4.30) 

wf(0)  =  2ti0.03 

(4.31) 

MO)  =  o 

(4.32) 

M0)  =  o 

(4.33) 

MO)  =  o 

( 4.34) 

M°)  =  o 

( 4.35) 

»(0)  =  diag  {6,5,3,2,0.08,0.01,0.01,0.01,0.01} 

(4.36) 

As  stated  in  Candy  [Ref.  4],  the  variation  of  the  gain  is  related  to  the  variations  in  P 
and  Q.  Uncertainty  in  the  model  can  be  characterized  by  the  process  noise  covariance,  Q. 
For  a  large  Q,  P  is  large,  which  indicates  a  high  uncertainty  or  an  inadequate  model.  For 
small  Q,  P  is  small,  indicating  an  adequate  model.  Therefore,  the  Kalman  gain  can  be 
thought  of  as  a  ratio  of  process  to  measurement  noise;  that  is, 
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G«§  (4.37) 


It  can  be  seen  that  the  variations  in  G  are  proportional  to  the  variations  of  Q.  The  Kal- 
man  estimator  can  be  perceived  as  a  deterministic  filter  with  a  time-varying  bandwidth  as 
determined  by  the  filter  gain.  As  Q  increases,  both  G  and  the  filter  bandwidth  increase. 
Thus,  the  filter  transient  performance  will  be  faster.  The  same  effect  can  be  observed  by  a 
small  R.  Conversely,  if  Q  decreases,  G  decreases,  which  decreases  the  bandwidth  of  the  fil- 
ter, and  hence,  the  filter  transient  response  is  slower. 

The  steady-state  value  of  the  error  covariance  matrix  P  increases  (decreases)  with  a 
corresponding  increasing  (decreasing)  value  of  the  Q  matrix.  We  conclude  that  P  is  also 
proportional  to  Q.  We  also  note  a  similar  effect  by  varying  R. 

We  investigated  the  effect  of  changing  the  initial  conditions  off  on  the  algorithm  per- 
formance. As  long  as  the  filter  is  stable  and  the  state-space  model  is  completely  controllable 
and  observable,  then 


limP(0  =P (small).  (  4.38) 

t  — >  °° 


Thus,  an  estimator  error  approaching  zero  implies  that  the  state  estimate  converges  to 
a  true  value,  given  that  enough  data  exists.  The  initial  estimates  will  affect  the  transient  per- 
formance of  the  algorithm,  since  a  large  initial  P(0)  gives  a  large  G(0) ,  and  therefore 
heavily  weights  the  initial  measurements  and  ignores  the  model. 

In  the  program,  we  used  R  =  0.1,  while 

Q  =  diag  {q^q^rf Yfi2,q2,q2^ 2# 2}  (4.39) 
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where 


q1  =  1  x  10"3  and  <72  =  1  x  10"7. 

The  entries  of  Q  and  R  roughly  determine  the  bandwidth  of  the  estimator.  This  also 
affects  the  capture  properties,  tracking  properties,  and  steady-state  error.  The  capture  prop- 
erty can  also  be  influenced  by  the  choice  of  P  (0) .  The  values  used  in  this  work  were  largely 
obtained  by  trial  and  error. 

Figure  4.6  shows  the  true  and  estimated  values  of  the  fundamental  frequency  from 
450  samples.  The  estimated  value  of  the  frequency  tracks  the  true  value  after  t  =  30  and 
locks  onto  it  very  closely.  Figure  4.2  shows  the  true  and  the  estimated  values  of  the  ampli- 
tude of  the  first  harmonic  component.  Tracking  is  achieved  after  t  =  50.  Figure  4.3  shows 
the  estimated  and  the  true  amplitudes  of  the  second  harmonic  component.  Tracking  is 
achieved  after  t  =  50,  although  the  initial  estimate  value  r2  (0)  is  zero.  The  other  harmonic 
amplitudes  and  their  estimates  show  similar  responses,  as  in  Figures  4.4  and  4.5. 

The  phase  error  of  the  fundamental  component  is  shown  in  Figure  4.7.  The  error  is 
close  to  zero  after  t  =  350.  A  similar  response  can  be  seen  in  Figure  4.8  for  the  phase  error 
of  the  second  harmonic.  Figure  4.9  shows  the  phase  error  of  the  third  harmonic.  At  t  =  450 , 
the  error  is  0.00025.  Figure  4.10  shows  the  phase  error  of  the  fourth  harmonic,  which  has  a 
steady  state  error  of  0.007  after  t  =  250.  The  phase  error  of  the  second  and  the  higher  har- 
monics is  higher  than  in  the  first  harmonic.  This  is  probably  because  the  initial  estimate  of 
the  second  and  the  higher  harmonic  amplitudes  were  zero. 

Since  higher  harmonics  are  assumed  to  be  negligible  we  only  considered  tracking  sig- 
nals with  less  than  four  harmonics.  For  one  case,  we  accepted  a  signal  consisting  of  two 
harmonics.  For  the  simulations,  we  let 


29 


2(0  =  rj sin  (2n0.04<  + 0.01) +r3sin  (67t0.04* +  0.01) +y(<)  (4.42) 

and  the  second  and  the  fourth  harmonics  are  assumed  to  be  missing.  The  filter  could  not 
initially  track  the  initial  error  covariance  matrix  used  with  four  harmonics.  So  we 
rearranged  the  initial  value  of  the  error  covariance  matrix  as 

P  (0)  =  diag{2,  1,  0.5,  0.2,  0.04,  0.01,  0.01,  0.01,  0.01}  .  (  4.43) 

We  used  the  MATLAB  program  in  Appendix  B  for  the  simulations.  The  results  are 
given  in  Figures  4.1 1  to  Figure  4.15.  Figures  4.1 1  and  4.12  show  the  true  and  estimated 
values  of  the  first  and  the  third  harmonics,  respectively.  For  both  cases  tracking  is  achieved 
after  t  =  30.  The  second  and  the  fourth  harmonic  true  and  estimated  values  were  around 
zero.  These  graphs  are  not  included.  Figure  4.13  shows  the  frequency  tracking  of  the  signal. 
Its  transient  response  was  similar  to  the  one  obtained  for  the  case  of  four  harmonics.  Figures 
4.14  and  4.15  show  the  phase  error  of  the  first  and  the  third  harmonics,  respectively.  The 
phase  error  of  the  third  harmonic  was  worse  than  the  first  harmonic  because  of  the  poor  ini- 
tial value  of  the  third  harmonic. 
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Figure  4.  2  True  and  the  estimate  value  of  the  amplitude  of  the  first  harmonic 
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Figure  4. 3  True  and  the  estimate  value  of  the  amplitude  of  the  second  harmonic 
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Figure  4. 4  True  and  the  estimate  value  of  the  amplitude  of  the  third  harmonic 
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Figure  4. 5  True  and  the  estimate  value  of  the  amplitude  of  the  fourth  harmonic 
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Figure  4. 6  True  and  the  estimate  value  of  the  fundamental  frequency 
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Figure  4. 7  Phaseerror  of  the  first  harmonic 
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Figure  4. 8  Phase  error  of  the  second  harmonic 
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Figure  4. 9  Phase  error  of  the  third  harmonic 
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Figure  4. 10  Phase  error  of  the  fourth  harmonic 
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Figure  4. 11  Amplitude  tracking  of  the  first  harmonic.  (Two  harmonic  case) 
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Figure  4. 12  Amplitude  tracking  of  the  third  harmonic.  (Two  harmonic  case) 
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Figure  4. 13  Frequency  tracking.  (Two  harmonic  case) 
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Figure  4. 14  Phase  error  of  the  first  harmonic.  (Two  harmonic  case) 
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Figure  4. 15  Phase  error  of  the  third  harmonic.  (Two  harmonic  case) 
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V.  RECTANGULAR  MODEL  OF  THE  SYSTEM 

This  chapter  applies  the  extended  Kalman  filter  to  the  signal  formulated  in  Chapter  IV, 
but  represents  it  in  a  different  way.  The  signal  is  not  periodic  but  has  frequency,  amplitude 
and  phases  that  change  slowly  over  time  in  a  similar  way  to  that  in  Chapter  IV.  Three  sets 
of  parameters  (frequency,  amplitude,  and  phase)  define  the  signal.  We  will  call  the 
parameterization  in  this  chapter  the  rectangular  model  as  defined  by  Anderson  and  Parker 
[Ref.  7]  and  Anderson  and  James  [Ref.  8].  In  this  representation  y  (t)  has  amplitudes  and 
phases  with  sine  and  cosine  components,  like 

y(t)   =  a  t  (t)  sin  (wf(t)t  +  <$>(t))  +  £  [ak(t)  sm  {k  (wf(t)t  +  <$>(t)))}   + 

k  =  2 

£  [bk(t)cos(k(wf(t)t  +  Q(t))))  (5.1) 


A  =  2 


The  parameters  of  the  system  are  ax  (t) ,  a2  (t)  ,...,am  (t) ,  b2  (t) ,  ...,bm(t) ,  wf(t) ,  and 
(0  .  The  signal  y{t)  has  slow  time  varying  frequency,  amplitudes,  and  phases.  That  is: 

wf=  wf{t)  (5.2) 

ak  =  ak(t)  (5.3) 

bk  =  bk(t)  (5.4) 

<j)  =  <}>(0  (5.5) 
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where  frequency,  amplitude,  and  phase  are  nearly  constant  over  several  cycles. 


\wf(t+  1)  -wAt)\ 

'=0  (5.6) 


=  0  (5.7) 

=  0  (5.8) 

sO  (5.9) 


wr(t) 

\ak{t 

+  D- 

a,(0| 

wf(t) 

\bk(t 

+  1)- 

MO| 

"y(0 

!♦« 

+  D- 

0(01. 

uv(*) 


In  the  representation  of  the  signal,  we  did  not  include  the  bx  (t)  term  cosinusoidal 
component  of  the  fundamental  frequency.  This  is  because  such  a  cosinusoidal  frequency 
would  give  a  phase  shift  in  the  fundamental,  represented  by  a  change  in  the  phase  of  the 
fundamental. 

Regardless  of  the  working  model,  we  will  assume  that  the  signal  is  contaminated  by 
additive  noise.  This  noise  v  (t)  will  be  white  and  Gaussian,  which  gives  the  measurement 
as: 

z{t)  =  y(t)  +v(t)  (5.10) 

From  these  measurements,  we  will  estimate  al(t),  a2(t),  ...,am(t),  b2(t), 
...,bm  (t)  ,wf(t)  ,ty(t).  The  estimation  will  be  carried  out  up  to  the  mth  harmonic.  The  higher 
harmonics  will  be  assumed  to  be  insignificant  for  our  purposes. 

A.   THE  ESTIMATOR 

The  extended  Kalman  filter  will  be  applied  for  the  estimation  of  frequency,  amplitude, 
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and  phase  of  the  measurement  signal.  In  a  similar  way,  we  will  denote  the  state-space 
model  of  the  rectangular  form.  Therefore,  Equations  (4.9)  and  (4.11)  remain  unchanged, 
while  the  others  become 


(0  =  [a^t)  &2(t)  ,...Am(t)  ,b2(t)  ,...,bm(t)  ,wf(t)  $  (t)] 


<£ 


~~ 

im    o 

0  0 

o  L-i 

0  0 

00 

1  0 

00 

0  1 

(5.11) 


(5.12) 


h(x(t))  =  a^t)  sin  ((wf(t)t  +  <^>(t)))  +  £  [ak(t)  sin  (k(wf(t)t +  <$>(t)))]  + 

k  =  2 

m 

£  [b k(t)  cos  (k(wf(t)t  +  <$>(t)  ))  ]  (5.13) 


k=2 


The  estimator  Equations  (4.18),  (4.19),  (4.20)  and  (4.21)  of  the  polar  form  remain  the 
same  as  for  the  rectangular  form.  The  only  difference  appears  in  a  Jacobian  matrix  as  writ- 
ten below: 


H(t)  = 


sin  ((wf(t\t-  1)  +$(t\t-  1)) 
sin  (2  (wf(t\t-  l)t  +  $(t\t-l))  ) 

sin(m(wf(t\t-l)t  +  ty(t\t-l))  ) 
cos  (2  (wf(t\t-  l)t  +  $(t\t-  1))  ) 


cos  (m  {wf(t\  t-l)t  +  $(t\t-l))  ) 
Z{t\t-1) 
P(t\t-D 


(5.14) 
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where, 


3  (t\t  -  1)  =   Y  kak  (t\  t  -  1)  t cos  (A  (uy(f|  f  -  1)  *  +  4>  (*|  *  -  1) )  )  - 


*  =  i 


Y  £6*  (*|  *  -  1)  t sin  (fc  (wf(t\  t-  l)t  +  <j>(*|  t-  1))) 


A  =  2 


and 


p(*|*-l)  =   V  kak(t\t-  1)  cos  (k(wf(t\  t-  l)t  +  4>  (f|f-l)) )  - 


A=  1 


V  kbk  (t\  t  -  1)  sin  (k  (wr(t\  t -  1)  t  +  4> (t\  t-  1)) ) . 


*  =  2 


B.     SIMULATIONS 

Like  the  polar  model,  the  rectangular  model  also  has  2m+\  states,  with  m  representing 
the  number  of  harmonics.  We  choose  the  number  of  harmonics  m=4  to  compare  with  the 
polar  model.  The  measurements  consisting  of  sine  and  cosine  components  are 

2(0  =  ax  (t)  sin  ( (2n0.04t  +  0.01 )  )  +a2  (t)  sin  (2  (2rc0.04*  +  0.01)  )  + 
a3  (0  sin  (3  (2rc0.04f  +  0.01) )  +a4  (t )  sin  (4  (2ti0.04*  +  0.01) )  + 
b2  (t)  cos  (2  (27t0.04*  +  0.01)  )  +b3  (0  cos  (3  (2tc0.04*  +  0.01) )  + 

b4(t)  cos  (4 (2nQ.04t  +  0.01))  +v  (t) .  (  5.15) 

I 

Here  v  (t)  is  zero  mean,  white  Gaussian  noise  with  a  variance  of  0.1.  The  signal  has 
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a  fundamental  frequency  wf  =  2rc0.04  and  amplitudes  of  ak  and  bk  for  the  sine  and  the  co- 
sine components,  respectively.  They  are  defined  as 


a 


k-  1 


*k  =  ^  *  =  2,3,4  (5.16) 


a 


k-\ 


6,  =  -^  *  =  2,3,4  (5.17) 

The  values  of  the  amplitudes  can  be  obtained  from  the  desired  signal  to  noise  ratio. 
The  program  written  in  MATLAB  (See  Appendix  C)  is  used  for  the  simulations  with  the 
following  initial  conditions: 


di  =  2.4 

(5.18) 

d2  =  0 

(5.19) 

d3  =  0 

(  5.20) 

d4  =  0 

(5.21) 

b2  =  1.45 

(  5.22) 

63  =  0 

(  5.23) 

64  =  0 

(  5.24) 

wf  =  2rc0.03 

(5.25) 

<j>  =  0 

(  5.26) 

P(0) 

=  diag{l,  0.5,  0.1,  0.04,  0.5, 

0.1, 

0.04, 

0.8, 

0.01} 

( 5.27) 

These  initial  values  are  chosen  closely  to  those  used  in  the  polar  form.  The  main  dif- 
ference appears  in  the  initial  value  of  the  error  covariance  matrix.  The  program  uses  the 
same  value  of  measurement  and  process  noise  as  in  the  polar  form. 
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Figure  5.1  shows  the  true  and  estimated  values  of  the  amplitude  of  the  first  harmonic. 
Tracking  is  achieved  after  t  =  50.  In  Figures  5.2  and  5.3,  the  amplitudes  of  the  sine  com- 
ponent of  the  second  and  third  harmonics  show  a  similar  response  to  the  first  one.  Figure 
5.4  shows  the  true  and  estimated  values  of  the  sine  component  of  the  fourth  harmonic.  The 
error  looks  worse  than  the  others  with  the  filter  tracking  after  t  =  150.  Figures  5.5, 5.6  and 
5.7  show  the  estimated  cosine  components.  Their  responses  are  similar  to  the  sine  compo- 
nents. Figure  5.8  shows  the  frequency  tracking  of  the  signal.  The  peak  value  of  the  estima- 
tion is  large  compared  to  the  one  estimated  in  the  polar  form.  But,  like  the  polar  form,  the 
rectangular  model  tracks  the  frequency  after  t  =  30  and  locks  onto  it  very  closely.  Figure 
5.9  shows  the  phase  error  of  the  signal.  It  has  a  steady  state  value  of  -0.0003  after  t  =  70. 
We  also  applied  a  case  with  less  than  four  harmonics  to  the  rectangular  model.  The 
measurements  are  assumed  to  be 

2(f)  =  ax  {t)  sin  (2*0.04*  +  0.01)  +a3  (t)  sin  (3  (2*0.04*  +  0.01) )  + 

b2  (t)  cos  (2  (2n0.04*  +  0.01) )  +v  (t) .  (  5.28) 

The  second  and  the  fourth  harmonics  of  the  sine  component  and  the  third  and  the 
fourth  harmonics  of  the  cosine  component  are  assumed  to  be  missing  for  the  rectangular 
model.  Similar  to  the  polar  form,  we  also  rearranged  the  error  covariance  matrix  for  the 
rectangular  form  as 


P(0)  =  diag  { 1,  0.2,  0.1,  0.02, 1,  0.1,  0.01,  0.1,  0.001} .  (  5.29) 

The  MATLAB  program  used  for  the  simulations  is  shown  in  Appendix  D.  Figure  5.K 
shows  the  true  and  estimated  values  of  the  amplitude  of  the  first  harmonic.  Its  response  i 
almost  similar  to  the  one  estimated  with  four  harmonics  in  the  rectangular  model.  Figur 
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5.11  gives  the  estimation  of  the  amplitude  of  the  sine  component  of  the  third  harmonic.  Fig- 
ure 5.12  shows  the  amplitude  tracking  of  the  cosine  component  of  the  second  harmonic, 
which  is  very  similar  to  the  one  obtained  in  the  four  harmonics  case.  The  true  and  estimated 
values  of  the  second  and  the  fourth  amplitude  harmonics  for  the  sine  component  were 
around  zero.  The  same  results  apply  to  the  third  and  the  fourth  amplitude  harmonics  of  the 
cosine  component.  These  graphs  were  not  included.  Figures  5.13  and  Figure  5.14  show  the 
frequency  tracking  and  the  phase  error  of  the  signal,  respectively. 
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Figure  5.1  True  and  estimate  value  of  the  amplitude.  First  harmonic 
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Figure  5.2  True  and  estimate  value  of  the  amplitude.  Sine  component,  second  harmonic 
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Figure  5.3  True  and  estimate  value  of  the  amplitude.  Sine  component,  third  harmonic 


Figure  5.4  True  and  estimate  value  of  the  amplitude.  Sine  component,  fourth  harmonic 
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Figure  5.5  True  and  estimate  value  of  the  amplitude:  Cosine  component,  second  harmoni 
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Figure  5.6  True  and  estimate  value  of  the  amplitude:  Cosine  component,  third  harmonic 
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Figure  5.7  True  and  estimate  value  of  the  amplitude.  Cosine  component,  fourth  harmonic 
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Figure  5.8  True  and  estimate  value  of  the  fundamental  frequency 
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Figure  5.9  Phase  error  of  the  signal 
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Figure  5.10  Amplitude  tracking  of  the  first  harmonic.  (Three  harmonic  case) 
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Figure  5.11  Amplitude  tracking.  Sine  component,  third  harmonic. (Three  harmonic  case) 
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Figure  5.12  Amplitude  tracking.  Cosine  component,  second  harmonic. 

(Three  harmonic  case) 
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Figure  5.13  Frequency  tracking.  (Three  harmonic  case) 


0.022 


0.006 
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VI.  CONCLUSIONS 

This  thesis  presents  the  estimation  of  frequency,  amplitude,  and  phase  of  a  signal  using 
an  extended  Kalman  filter  algorithm.  Both  the  polar  and  the  rectangular  model  filter  were 
implemented.  In  both  models,  the  signal  is  contaminated  by  noise. 

The  simulations  show  that  the  performance  of  both  filters  are  similar.  Although  there 
is  little  difference  between  the  two  models,  the  rectangular  model  seems  to  be  able  to 
estimate  phase  better  than  the  polar  model.  For  both  filters,  the  tracking  of  the  fundamental 
frequency  plays  a  significant  role.  If  the  estimated  value  of  the  frequency  locks  onto  a 
multiple  of  the  true  frequency,  the  filter  cannot  track  the  amplitudes.  Each  model  offers  a 
different  advantage.  The  estimation  of  amplitudes  of  the  signal  is  more  convenient  to 
measure  in  the  polar  model  than  in  the  rectangular  model. 

In  all  the  simulations,  an  18  dB  signal-to-noise  ratio  was  used.  The  performance  of  the 
two  filters  was  poor  when  lower  signal-to-noise  ratios  were  used.  The  signals  may  have  a 
Doppler  shift.  By  adding  another  state,  additional  Doppler  effects  can  be  accounted  for  in 
future  work. 
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APPENDIX    A 


%This  program  tracks  the  frequency,  amplitude,  and  the 
phase  of  the  nonperiodic  signal  in  noise  by  using  extended 
Kalman  filter  algorithm  for  the  polar  model. 

%  State  space  model 

F= [100000000 
010000000 
001000000 
000100000 
000010000 
000001000 
000000100 
000000010 
000000001]; 


%  Initial  conditions  of  the  true  state 

rl(l)=2.8;  %Amplitude  of  the  first  component 

r2  (1) =rl  (1 ) /2;  %Amplitude  of  the  second  component 

r3  (1 ) =r2  (1 ) /2;  %Amplitude  of  the  third  component 

r4  (1 ) =r3  (1 ) /2;  %Amplitude  of  the  fourth  component 

01(1) =0.01;  %Phase  of  the  first  component 

02(1) =0.01;  %Phase  of  the  second  component 

03(1) =0.01;  %Phase  of  the  third  component 

04(1) =0.01;  %Phase  of  the  fourth  component 

w  (1) =2*pi*0 . 04;  %Fundametal  frequency 


%Initial  condition  of  the  estimation 
xkkm ( : , 1 )  =  [ 2 . 6 ; 0 ; 0 ; 0 ; 2  *pi * 0 . 0 3 ; 0 ; 0 ;  0 ;  0 ]  ; 

R=0.1;  %  measurement  noise 
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ql=le-3;    %   process    noise 

q2=le-7; 

q3=le-7; 

Q=[ql    0    0    0    0    0    0    0    0 

Oql    0000000 

00    ql    000000 

000    ql    00000 

0000    q2    0000 

00000    q3    000 

000000    q3    00 

0000000    q3    0 

00000000    q3]  ; 

%Error    covariance   matrix 
P=[6    00000000 

050000000 

003000000 

000200000 

0    0    0    0    0.08    0    0    0    0 

0    0    0    0    0    0.001    0    0    0 

0    0    0    0    0    0    0.001    0    0 

0    0    0    0    0    0    0    0.001    0 

000000000.001]; 

%Jacobian   matrix 

H  ( 1 ,  : )  =  [  0    0    0    0    0    0    0    0    0  ]  ; 

^Transpose  of  the  Jacobian  matrix 
Ht(:,l)=H(l,  :)'  ; 

t=l:450;  %Number  of  samples 

%True  state  matrix 

x(:,l)  =  [rl(l) 

r2(l) 

r3(l) 

r4(l) 

w(l) 

01(1) 

02(1) 

03(1) 
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04(1)]; 

for  i  =  1:450 
rand ( 'normal' ) 

%Observation 
z(i)=x(l,i)*sin(x(5,i)*i+x(6,i))  + 

x(2,i)*sin(2*x(5,i)*i  +  x(7,i))+x(3,i)*sin(3*x(5,i)*i  +  x(8,i))  + 
x(4,i)*sin(4*x(5,i)*i  +  x(9,i))+0. l*rand(l) ; 


he (i) =xkkm (1, i) *sin (xkkm (5, i) *i+xkkm (6, i) ) +xkkm (2,i)*sin(2*x 
kkm (5, i) *i+xkkm(7, i) ) +xkkm (3, i) *sin (xkkm (5, i) *3*i+xkkm (8, i) ) 
+  xkkm (4, i) *sin (xkkm (5, i) *4*i  +  xkkm (9,  i)  )  ; 

%  Kalman  gain 

L(:,i)=P*H(i,  :)  '  *inv(H(i,  :  )  *P*H(i,  : )  ' +R)  ; 

%  Estimate  update 

xkk ( : , i) =xkkm( : , i) +L ( : , i) *  (z (i)  -  he (i) )  ; 

xkkm( : ,i+l) =F*xkk ( :  ,  i)  ; 

%Tranpose  of  the  Jacobian  matrix 

Ht (:, i+1) =[sin (xkkm (5, i) *i+xkkm(6, i) ) ; 

sin (2*xkkm (5, i) *i  +  xkkm (7,  i  )  )  ; 

sin  (xkkm (5, i) *3*i+xkkm(8,  i)  )  ; 

sin (xkkm (5, i) *4*i+xkkm  (9, i)  )  ; 

xkkm (1, i) *i*cos (xkkm (5, i) *i+xkkm (6, i) ) +xkkm (2,i)*2*i*cos(2*x 
kkm (5, i) *i+xkkm (7, i) ) 

+xkkm (3,i)*3*i*cos (3*xkkm (5, i) *i+xkkm (8, i) ) +  xkkm (4, i) *4*i*co 
s (4*xkkm(5, i) *i+xkkm(9, i)  )  ; 

xkkm (1, i) *cos (xkkm (5, i) *i+xkkm (6, i) ) ; 

xkkm (2, i) *cos (2* xkkm (5, i) *i+xkkm (7, i) ) ; 

xkkm  (3, i) *cos  (3*xkkm (5, i) *i  +  xkkm (8,  i)  ) / 

xkkm (4,  i) *cos (4*xkkm (5, i) *i+xkkm (9, i) ) ] ; 

%Jacobian 

H(i+1,  :)=Ht (:,i+l)  '  ; 

%  Error  covariance  update 
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P=F* (P-L ( : , i) *H  (i,  : ) *P) *F' +    Q; 

v(:,i)  =  [randd) *le-2 
rand(l) *le-2 
rancid)  *le-2 
randd)  *le-2 
rand(l) *le-4 
randd)  *le-4 
randd)  *le-4 
randd)  *le-4 
randd)  *le-4]  ; 

%State  update 
x(:,i+l)=F*x(:,  i)  +  v(:,i)  ; 

end 

plot  (t,  x  (1,  1  :i)  ,  t,  xkk  (1,  1  :i)  )  ,  grid,  xlabel  (  'Time'  )  , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext ( '--Estimate 
of  the  State' ),meta  tezlal 

plot(t,x(2,l:i),t, xkk (2, 1 : i) ) /pause, grid, xlabel ( 'Time'  )  , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext (  '--Estimate 
of  the  State' ),meta  tezla2 

plot  (t,x(3,  1  :i)  ,  t ,  xkk  ('3,  l:i)  )  /pause,  grid,  xlabel  (  'Time'  )  , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext ( ' — Estimate 
of  the  State' ),meta  tezla3 

plot(t,x(4,l:i),t, xkk  (4, 1 : i) ) /pause, grid, xlabel ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext (  '--Estimate 
of  the  State' ),meta  tezla4 

plot (t,x(5, l:i) ,t,xkk(5, 1 :i) ) /pause, grid, xlabel ( 'Time' ) , 
ylabel (  'Frequency' ) , gtext ( '-True  State' ) , gtext ( '--Estimate 
of  the  State' ),meta  tezla5 

plot(t,x(6,l:i) -xkk (6, 1 : i) ) /pause, grid, 

xlabel ( 'Time' ), ylabel ( 'Phase  Error' ),meta  tezla6 

plot(t,x(7,l:i) -xkk  (7, 1 : i) ) /pause, grid, 

xlabel ( 'Time' ), ylabel ( 'Phase  Eror'),meta  tezla7 
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plot  (t ,  x  (8,  1 :  i)  -xkk  (8,  1  :i)  )  ,-pause,  grid, 

xlabel  (  'Time' ), ylabel  (  'Phase  Error' ), meta  tezla8 

plot(t,x(9,l:i)-xkk(9,l:i) ) /pause, grid, 

xlabel ( 'Time' ), ylabel ( 'Phase  Error'), meta  tezla9 
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APPENDIX    B 


%This  program  computes  the  frequency,  amplitude  and  the 
phase  of  the, nonperiodic  signal  in  noise  which  consists  of 
two  harmonics  for  the  polar  model. It  uses  extended  Kalman 
filter  algorithm. 

%  State  space  model 

F= [100000000 
010000000 
001000000 
000100000 
000010000 
000001000 
000000100 
000000010 
000000001]; 

%  Initial  conditions  of  the  true  state 

rl(l)=2.8;  %Amplitude  of  the  first  component 

r2  (1) =0; %rl  (1) /2;  %Amplitude  of  the  second  component 

r3 (1 ) =rl (1 ) /4 ;  %Amplitude  of  the  third  component 

r4 (1)  =0;  %r3 (1) /2;  %Amplitude  of  the  fourth  component 

01(1) =0.01;  %Phase  of  the  first  component 

02  (1) =0; %0 . 01/  %Phase  of  the  second  component 

03(1) =0.01;  %Phase  of  the  third  component 

04 (1)=0;%0.01;  %Phase  of  the  fourth  component 

w (1) =2*pi*0 . 04;  %Fundametal  frequency 

%Initial  condition  of  the  estimation 
xkkm ( : , 1 )  =  [ 2 . 6 ; 0 ; 0 ; 0 ; 2  *pi  *  0 . 02 ; 0 ; 0 ; 0 ; 0 ] ; 

R=0.1;  %  measurement  noise 
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ql=le-3;    %   process    noise 

q2=le-7; 

q3=le-7; 

Q=[ql    0    0    0    0    0    0    0    0 

Oql    0000000 

00    ql    000000 

000    ql    00000 

0000    q2    0000 

00000    q3    000 

000000    q3    00 

0000000    q3    0 

00000000    q3]; 

%Error    covariance   matrix 
P=[2    00000000 

010000000 

00    0.5    000000 

000    0.2    00000 

0    0    0    0    0.04    0    0    0    0 

0    0    0    0    0    0.001    0    0    0 

0    0    0    0    0    0    0.001    0    0 

0    0    0    0    0    0    0    0.001    0 

0    0    0    0    0    0    0    0    0.001]; 

%Jacobian   matrix 

H  ( 1 ,  : )  =  [  0    0    0    0    0    0    0    0    0  ]  ; 

%Transpose  of  the  Jacobian  matrix 
Ht ( : , 1 ) =H  ( 1 ,  : ) ' ; 

t=l:450; 

%True  state  matrix 

x(:,l)  =  [rl(l) 

r2(l) 

r3(l) 

r4  (1) 

w(l) 

01  (1) 

02(1) 
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03(1) 
04(1)]; 

for  i  =  1:450 
rand ( 'normal' ) 

%0bservation 

z  (i) =x  (1,  i)  *sin(x(5/  i)  *i+x  (6,  i)  )  + 
x(2,i)*sin(2*x(5/i)*i  +  x(7,i))+x(3/i)*sin(3*x(5,i)*i  +  x(8,i))  + 
x(4,i)*sin(4*x(5,i)*i  +  x(9,i))  +0  .  l*rand  (1)  ; 


he (i) =xkkm (1, i) *sin (xkkm (5, i) *i+xkkm(6, i) ) +xkkm(2, i) *sin (2*x 
kkm(5, i) *i+xkkm(7, i) ) +xkkm(3, i) *sin (xkkm(5,  i) *3*i  +  xkkm (8,  i) ) 
+xkkm (4, i) *sin (xkkm (5, i) *4*i+xkkm (9, i) ) ; 

%  Kalman  gain 

L(:,i)=P*H(i,  :)  '  *inv(H(i,  :) *P*H(i,  : )  ' +R)  ; 


%  Estimate  update 

xkk  (:,  i)  =xkkm(  :,i)+L(:,i)*(z(i) 

xkkm( : , i+1) =F*xkk (:,i)  ; 


-  he(i)  )  ; 


%Tranpose  of  the  Jacobian  matrix 

Ht (: , i+1)  =  [sin  (xkkm (5, i) *i+xkkm (6, i) ) ; 

sin  (2*xkkm(5, i) *i+xkkm (7,  i)  ) 

sin (xkkm (5, i) *3*i  +  xkkm (8,  i)  ) 

sin  (xkkm (5, i) *4*i+xkkm (9,  i)  ) 


xkkm  (1, i) *i*cos  (xkkm (5, i) *i+xkkm (6, i) ) +xkkm (2,i)*2*i*cos(2*x 
kkm(5, i) *i+xkkm(7, i) ) . . 

+xkkm (3,i)*3*i*cos (3*xkkm (5, i) *i+xkkm(8,  i) ) +  xkkm (4,i)*4*i*co 
s  (4*xkkm(5, i) *i+xkkm(9,  i)  )  ; 

xkkm (1, i) *cos (xkkm (5, i) *i+xkkm (6, i) ) ; 

xkkm (2, i) *cos (2*xkkm(5, i) *i+xkkm(7, i) ) ; 

xkkm (3, i) *cos (3*xkkm (5, i) *i+xkkm (8,  i) )  ; 

xkkm (4, i) *cos (4*xkkm (5, i) *i+xkkm (9, i)  )  ]  ; 

%Jacobian 

H(i+1,  :)=Ht(:,i+l)  '  ; 
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%  Error  covariance  update 
P=F*  (P-L(  : , i) *H  (i,  : ) *P)  *F' +  Q; 

v(:, i)=[rand(l) *le-2 
rand(l) *le-2 
rand(l) *le-2 
rand(l) *le-2 
rand(l) *le-4 
rand(l) *le-4 
rand(l) *le-4 
rand(l) *le-4 
rand(l) *le-4] ; 

%State  update 

x(:,  i  +  l)=F*x  (:,i)  +  v(:,i)  ; 

end 

plot (t , x (1 , 1 : i) , t , xkk (1 , 1 : i) ) /pause, grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext (  '--Estimate 
of  the  State' ), meta  tezlbl 

plot(t,x(2,l:i) ,t,xkk(2,l:i) ) /pause, grid, x label ( 'Time'  )  , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext (  '--Estimate 
of  the  State'), meta  tezlb2 

plot  (t,x(3, 1 : i) ,t,xkk(3, 1 : i) ) /pause, grid, x label ( 'Time'  )  , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext (  '--Estimate 
of  the  State'), meta  tezlb3 

plot  (t, x  (4, 1 : i) , t, xkk  (4, 1 : i)  ) /pause, grid, x label (  'Time' )  , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext (  '--Estimate 
of  the  State'), meta  tezlb4 

plot  (t,x(5, 1 : i) ,t,xkk(5, 1 : i) ) /pause, grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext ( '--Estimate 
of  the  State'), meta  tezlb5 

plot(t,x(6,l:i)-xkk(6, 1 :i) ) /pause, grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext (  '--Estimate 
of  the  State'), meta  tezlb6 

plot (t, x (7, 1 : i) -xkk (7, 1 : i) ) /pause, grid, xlabel ( 'Time' ) , 
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ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext ( * — Estimate 
of  the  State' ),meta  tezlb7 

plot(t,x(8,l:i) -xkk  (8, 1 :i) ) /pause, grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext (  '--Estimate 
of  the  State' ),meta  tezlb8 

plot(t,x(9,l:i) -xkk  (9, 1 :i) ) ; pause, grid, xlabel ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext ( '--Estimate 
of  the  State' ),meta  tezlb9 
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APPENDIX    C 

%This  program  trackes  the  frequency,  amplitude,  and  the  phase  of  the  nonperiodic 

signal  in  noise  by  using  extended  Kalman  filter  algorithm  for  the  rectangular  model. 

%  State  space  model 

F= [100000000 
010000000 
001000000 
000100000 
000010000 
000001000 
000000100 
000000010 
000000001]; 

%  Initial  conditions  of  the  true  state 
a  (1 ) =2 . 7; %Amplitude  of  the  first  component. 
a2  (1 ) =a  (1) /2; %Amplitude  of  the  second  component. 
a3 (1 ) =a (2) /2; %Amplitude  of  the  third  component. 
a4 (1) =a (3) 12;  %Amplitude  of  the  fourth  component. 
w(l)=2*pi*0.04; %Fundamental  frequency. 
01 (1) =0 . 01;  %Phase  of  the  first  component 
b2 (1) =a (1 ) /2; %Phase  of  the  second  component 
b3 (1) =a (2) /2;%Phase  of  the  third  component 
b4 (1) =a (2) 12;  %Phase  of  the  fourth  component 

%Initial  condition  of  the  estimation 
xkkm( :,1) = [2 . 4 ; 0; 0; 0; 1 . 4  50; 0; 0; 2*pi*0 . 03; 0 ] ; 

R=0.1;  %  Measurement  noise 

ql=le-3;  %  process  noise 

q2=le-7; 

q3=le-7; 
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Q=[ql  0  0  0  0  0  0  0  0 
Oql  0000000 
00  ql  000000 
000  ql  00000 
0000  ql  0000 
00000  ql  000 
000000  ql  00 
0000000  q2  0 
00000000    q3] ; 

%Error   covariance   matrix. 
Pkk=[l    00000000 

0  0. 5  0000000 
00  0.1  000000 
0  0  0  0.04  0  0  0  0  0 
0  0  0  0  0.5  0  0  0  0 
00000  0.1  000 
0  0  0  0  0  0  0.04  0  0 
0000000  0. 8  0 
0    0    0    0    0    0    0    0    0.001]; 

%Jacobian   matrix. 
H  ( 1 ,  : )  =  [  0    0    0    0    0    0    0    0    0  ]  ; 

%Transpose    of   the    Jacobian   matrix 
Ht  ( : ,  1 )  =  [  0    0    0    0    0    0    0    0    0  ]  '  ; 

t=l : 450; %Number    of    samples 

%True    state   matrix 
x(:,l)  =  [a(l) 
a2(l) 
a3(l) 
a4(l) 
b2(l) 
b3(l) 
b4(l) 
w(l) 
01(1)]; 
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for  i=  1:450 
rand ( ^normal '  ) 


z(i)=x(l,i)*sin(x(8,i)*i  +  x(9,i))+x(2,i)*sin(2*(x(8,i)*i  +  x(9, 
i)))+x(3,i)*sin(3*(x(8,i)*i  +  x(9,i)))+x(4,i)*sin(4*(i*x(8,i)  + 
x(9,i)))+x(5,i) *cos  (2*  (x  (8,  i)  *i+x  (9,  i)  )  ) +x  (6,  i) *cos (3* (x (8,  i 
) *i+x(9,i) ) )+x(7,i) *cos (4*(x(8,i)*i+x(9,i))) +0.1*rand(l) ; 

%Variable 

je (i) =xkkm(l, i) *sin (xkkm (8, i) *i+xkkm(9, i) ) +xkkm(2, i) *sin (2* ( 
xkkm  (8, i) *i+xkkm(9/ i) ) ) +xkkm(3, i) *sin (3* (xkkm (8, i) *i+xkkm(9, 
i) ) ) +xkkm(4, i) *sin  (4* (i*xkkm(8, i) +xkkm(9,  i) ) ) ; 

%Variable 

ke (i) =xkkm (5, i) *cos (2* (xkkm (8, i) *i+xkkm (9, i) ) ) +  xkkm (6, i) *cos 
(3* (xkkm (8, i) *i  +  xkkm(9, i)  )  ) +xkkm(7,  i) *cos (4*  (xkkm (8, i) *i  +  xkk 
m(9,i)  )  )  ; 

he (i)=je  (i) +ke  (i) ; 

%Time  update  of  the  error  covariance 
Pkkm=F*Pkk*F' +Q; 

%  Kalman  gain 
K(:, i)=Pkkm*H(i, : ) ' *inv(H(i, : ) *Pkkm*H(i, :) '  +R) ; 

%  Estimate  update 
xkk(:,i)=xkkm(:,i)+K(:,i)  *  (z  (i)  -  he(i)); 
xkkm( :, i+1) =F*xkk (:,i) ; 

%Variable 

c (i) =xkkm(l, i) *i*cos (xkkm (8, i) *i+xkkm(9, i) ) +xkkm(2, i) *2*i*co 
s (2* (xkkm (8, i) *i+xkkm(9, i) ) ) +xkkm(3, i) *3*i*cos (3* (xkkm (8,  i) * 
i  +  xkkm(9, i) )  ) +xkkm (4, i) *4*i*cos (4* (i*xkkm(8, i) +xkkm(9,  i) ) )  ; 

%Variable 

d(i) =-xkkm(5, i)*2*i*sin(2* (xkkm (8, i) *i+xkkm(9, i) ) )- 
xkkm (6, i)*3*i*sin(3* (xkkm (8, i) *i+xkkm(9,  i) ) )- 
xkkm  (7, i)*4*i*sin(4* (xkkm (8, i) *i+xkkm(9,  i) ) )  ; 
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%Variable 

f (i) =xkkm(l, i) *cos (xkkm(8, i) *i+xkkm(9, i) ) +xkkm(2, i) *2*cos (2* 
(xkkm(8, i) *i+xkkm(9, i) ) ) +xkkm(3, i) *3*cos (3* (xkkm(8, i) *i+xkkm 
(9, i) ) ) +xkkm(4, i) *4*cos (4* (i*xkkm(8, i) +xkkm(9,  i)  )  )  ; 
g(i) =-xkkm(5, i)*2*sin(2* (xkkm(8, i) *i+xkkm(9, i) ) ) - 
xkkm(6, i)*3*sin(3* (xkkm(8, i) *i+xkkm(9,  i)  )  )  - 
xkkm(7, i)*4*sin(4* (xkkm(8, i) *i+xkkm(9,  i) ) ) ; 


%transpose  of  the  Jacobian 
Ht  (:,i+l)  =  [sin (xkkm(8, i) *i+xkkm(9,  i)  )  ; 
sin (2* (xkkm(8, i) *i+xkkm(9, i) 
sin (3* (xkkm(8, i) *i+xkkm(9, i) 
sin (4* (xkkm(8, i) *i+xkkm(9, i) 
cos (2* (xkkm(8, i) *i+xkkm(9, i) 
cos (3* (xkkm(8, i) *i+xkkm(9, i) 
cos (4* (xkkm(8, i) *i+xkkm(9, i) 
c(i)+d(i)  ; 
f  (i)+g(i)  ]; 


%Jacobian 
H(i+1,  : )=Ht  (:, i+1) ' ; 

^Measurement  update  of  error  covariance 
Pkk=(eye(9)  -  K ( : , i) *H (i, : ) ) *Pkkm; 


v(:,i)  = 


[rand(l) *le-2 
rand(l) *le-2 
rand(l) *le-2 
rand(l) *le-2 
rand(l) *le-2 
rand(l) *le-2 
rand(l) *le-2 
rand(l) *le-4 
rand(l) *le-4] ; 


%State  update 
x  (:,  i  +  l)=F*x  (:,i)  +  v(:,i)  ; 

end 

plot  (t,  x  (1,  1 :  i) , t,xkk  (1, l:i) ) ,grid,xlabel ( 'Time'  )  , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) ,  gtext ( ' — Estimate 
of  the  State' ),meta  tez52cl 
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plot  (t ,  x (2, 1 : i) , t , xkk  (2, 1 : i) ) /pause, grid, x label ( 'Time'  )  , 
ylabel (  'Amplitude' ) , gtext ( '-True  State' ) , gtext ( '--Estimate 
of  the  State' ), met a  tez52c2 

plot  (t,x(3, 1 : i) ,t,xkk(3, 1 : i) ) /pause, grid, x label ( 'Time'  )  , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext (  '--Estimate 
of  the  State' ),meta  tez52c3 

plot(t,x(4,l:i),t, xkk  (4, 1 : i) ) /pause, grid, x label ( 'Time'  )  , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext ( '--Estimate 
of  the  State' ),meta  tez52c4 

plot  (t,x(5, 1 : i)  , t , xkk  (5, l:i) ) /pause, grid, x label ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext ( '--Estimate 
of  the  State' ),meta  tez52c5 

plot(t,x(6,l:i) ,t,xkk(6,l:i) ) /pause, grid, x label ( 'Time'  )  , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext (  '--Estimate 
of  the  State' ),meta  tez52c6 

plot  (t , x  (7, 1 : i) , t , xkk (7, 1 : i) ) /pause, grid, x label ( 'Time'  )  , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext (  '--Estimate 
of  the  State' ),meta  tez52c7 

plot(t,x(8,l:i)-xkk(8,l:i) ) /pause, grid, x label ( 'Time'  )  , 
ylabel ( 'Phase  Eror' ), gtext ( '--Estimate  of  the  State' ),meta 
tez52c8 

plot(t,x(9,l:i) ,t,xkk(9,l:i) ) /pause, grid, 
xlabel ( 'Time' ), ylabel ( 'Phase  Eror'),meta  tezla9 
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APPENDIX    D 

%This  program  trackes  the  frequency,  amplitude,  and  the  phase  of  the  nonperiodic 
signal  in  noise  which  consists  of  two  harmonics.  It  uses  extended  Kalman  filter  algorithm 
for  the  rectangular  model. 

%  State  space  model 

F= [100000000 
010000000 
001000000 
000100000 
000010000 
000001000 
000000100 
000000010 
000000001]; 

%  Initial  conditions  of  the  true  state 
a (1) -2 . 7; %Amplitude  of  the  first  component. 
a2 (1) =0; %Amplitude  of  the  second  component. 
a3 (1) =a (2) /2; %Amplitude  of  the  third  component. 
a4 (1 ) =0; %Amplitude  of  the  fourth  component. 
w(l)=2*pi*0.04; %Fundamental  frequency. 
01  (1) =0 . 01; %Phase  of  the  first  component 
b2 (1) =a (1) /2; %Phase  of  the  second  component 
b3 (1) =0; %Phase  of  the  third  component 
b4 (1) =0; %Phase  of  the  fourth  component 

%Initial  condition  of  the  estimation 
xkkm  ( : , 1 )  =  [ 2 . 4 ; 0 ; 0 ; 0 ; 1 . 4 5 0 ; 0 ; 0 ; 2  *pi *  0 . 0 3 ; 0 ]  ; 

R=0.1;  %  Measurement  noise 

ql=le-3;  %  process  noise 
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q2=le-7; 
q3=le-7; 
Q=[ql    0    0    0    0    0    0    0    0 

Oql    0000000 

00    ql    000000 

000    ql    00000 

0000    ql    0000 

00000    ql    000 

000000    ql    00 

0000000    q2    0 

00000000    q3]  ; 

%Error    covariance   matrix. 
Pkk=[l    00000000 

0    0.2    0    0    0    0    0    0    0 
00    0.1    000000 
0    0    0    0.02    0    0    0    0    0 
000010000 
00000    0.1    000 
0    0    0    0    0    0    0.01    0    0 
0000000    0. 10 
0    0    0    0    0    0    0    0    0.001]; 

%Jacobian   matrix. 
H(1,:)  =  [0    0    0    0    0    0    0    0    0  ]  ; 

^Transpose    of   the    Jacobian   matrix 
Ht  ( : ,  1 )  =  [  0    0    0    0    0    0    0    0    0  ]  '  ; 

t=l : 450; %Number    of    samples 

%True    state   matrix 
x(:/l)  =  [a(l) 
a2(l) 
a3(l) 
a4  (1) 
b2(l) 
b3(l) 
b4  (1) 
w(l) 
01(1)]; 
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for  i=  1:450 
rand ( 'normal' ) 


z(i)=x(l,i)*sin(x(8,i)*i  +  x(9,i))+x(2,i)*sin(2*(x(8,i)  *i+x(9, 
i)))+x(3,i)*sin(3*(x(8,i) *i+x(9, i)  )  ) +x  (4,  i)  *sin  (4*  (i*x  (8,  i)  + 
x(9,i)))+x(5/i) *cos (2* (x(8, i) *i  +  x ( 9, i) ) ) +x ( 6, i) *cos  (3* (x(8,i 
) *i+x  (9, i) ) ) +x(7, i) *cos (4* (x (8, i) *i+x (9, i) ) ) +0 . l*rand (1) ; 

%Variable 

je (i) =xkkm(l, i) *sin (xkkm(8, i) *i+xkkm(9, i) ) +xkkm(2, i) *sin  (2* ( 
xkkm(8, i) *i+xkkm(9, i) ) ) +xkkm(3, i) *sin  (3* (xkkm(8, i) *i+xkkm(9, 
i) ) ) +xkkm(4, i) *sin (4* (i*xkkm(8, i) +xkkm(9, i) ) ) ; 

%Variable 

ke (i) =xkkm(5, i) *cos  (2*  (xkkm(8, i) *i+xkkm(9, i) ) ) +xkkm(6, i) *cos 
(3* (xkkm(8, i) *i+xkkm(9, i) ) ) +xkkm(7, i) *cos (4* (xkkm(8, i) *i+xkk 
m(9,i)  )  )  ; 

he (i)=je (i) +ke  (i) ; 

%Time  update  of  the  error  covariance 
Pkkm=F*Pkk*F' +Q; 

%  Kalman  gain 
K(:,i) =Pkkm*H  (i,  : ) ' *inv(H (i,  :  ) *Pkkm*H(i,  :) '  +R) ; 

%  Estimate  update 
xkk  ( : , i) =xkkm( : , i) +K ( : , i) * (z (i)  -  he(i))/ 
xkkm( : , i+1) =F*xkk (:,i) ; 

%Variable 

c (i) =xkkm (1, i) *i*cos (xkkm(8, i) *i+xkkm(9, i) ) +xkkm(2, i) *2*i*co 
s (2* (xkkm(8, i) *i+xkkm(9, i) ) ) +xkkm(3, i) *3*i*cos (3* (xkkm(8,i) * 
i+xkkm(9, i) ) ) +xkkm(4, i) *4*i*cos (4* (i*xkkm(8, i) +xkkm(9, i) ) ) ; 

%Variable 
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d(i) =-xkkm(5/ i)*2*i*sin(2* (xkkm (8, i) *i+xkkm(9, i) ) )- 
xkkm (6, i)*3*i*sin(3* (xkkm(8, i) *i+xkkm(9,  i)  )  )- 
xkkm (7, i)*4*i*sin(4* (xkkm (8, i) *i+xkkm (9, i)  )  )  ; 

%Variable 

f (i) =xkkm(l, i) *cos (xkkm  (8, i) *i  +  xkkm ( 9, i) ) +xkkm(2, i) *2*cos  (2* 
(xkkm (8, i) *i+xkkm(9, i) ) ) +xkkm(3, i) *3*cos (3* (xkkm (8, i) *i+xkkm 
(9,  i)  )  )  +xkkm(4, i)*4*cos(4* (i*xkkm(8, i) +xkkm(9, i) ) ) ; 
g (i) =-xkkm(5, i)*2*sin(2* (xkkm (8, i) *i+xkkm(9, i) ) ) - 
xkkm (6, i)*3*sin(3* (xkkm (8, i) *i+xkkm(9,  i) ) ) - 
xkkm (7, i)*4*sin(4* (xkkm (8, i) *i+xkkm(9,  i)  )  )  ; 


^transpose  of  the  Jacobian 
Ht  (  : , i  +  1)  =  [sin  (xkkm (8, i) *i+xkkm(9,  i)  )  ; 
sin (2* (xkkm (8, i) *i+xkkm(9, i) 
sin (3* (xkkm (8, i) *i+xkkm(9, i) 
sin (4* (xkkm (8, i) *i+xkkm (9,  i) 
cos (2* (xkkm (8, i) *i+xkkm(9, i) 
cos (3* (xkkm (8, i) *i+xkkm(9, i) 
cos (4* (xkkm (8, i) *i+xkkm (9, i) 
c(i)+d(i)  ; 
f  (i)+g(i)  ]; 


%Jacobian 
H(i+1,  :)=Ht  (:,  i  +  1)  '  ; 

^Measurement  update  of  error  covariance 
Pkk=(eye(9)  -  K  (  : , i) *H  (i,  : ) ) *Pkkm; 

v(:,i)  =  [rand(l) *le-2 
rand(l) *le-2 
rand(l) *le-2 
rand(l) *le-2 
rand(l) *le-2 
rand(l) *le-2 
rand(l) *le-2 
rand(l) *le-4 
rand(l) *le-4] ; 

%State  update 
x(:,  i+l)=F*x(:f  i)  +  v(:,i)  ; 
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end 

plot  (t,x(l,  l:i)  ,  t,  xkk  (1, 1 :  i)  )  ,  grid,  x  label  (  'Time'  )  , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext ( '--Estimate 
of  the  State' ),meta  tez52cl 

plot(t,x(2,l:i),t, xkk  (2, 1 : i) ) /pause, grid, xlabel ( 'Time' )  , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext ( '--Estimate 
of  the  State' ),meta  tez52c2 

plot(t,x(3,l:i),t, xkk (3, 1 : i) ) ; pause, grid, xlabel ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' )  ,  gtext ( ' — Estimate 

of  the  State' ),meta  tez52c3 

plot (t, x (4, 1 :i) , t, xkk (4, l:i) ) ; pause, grid, xlabel ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext ( '--Estimate 
of  the  State' ),meta  tez52c4 

plot(t,x(5,l:i),t, xkk  (5, 1 : i) ) ; pause, grid, xlabel ( 'Time'  ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext ( ' — Estimate 
of  the  State' ),meta  tez52c5 

plot  (t , x  (6, l:i) , t , xkk  (6, 1 : i) ) /pause, grid, xlabel ( 'Time'  )  , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext (  '--Estimate 
of  the  State' ),meta  tez52c6 

plot  (t, x  (7, l:i) , t, xkk  (7, l:i) ) /pause, grid, xlabel ( 'Time' ) , 
ylabel ( 'Amplitude' ) , gtext ( '-True  State' ) , gtext (  '--Estimate 
of  the  State' ),meta  tez52c7 

plot  (t, x  (8, 1 : i) -xkk  (8, 1 :i) ) /pause, grid, xlabel ( 'Time' ) , 
ylabel (  'Frequency' ), gtext ( '--Estimate  of  the  State' ),meta 
tez52c8 

plot(t,x(9,l:i),t, xkk (9, 1 : i) ) /pause, grid, 

xlabel ( 'Time' ), ylabel ( 'Phase  Eror'),meta  tez52c9 
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